// File:          first_webots_ros_simulation.cpp
// Date:
// Description:
// Author:          liuyn
// Modifications:

#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/NavSatFix.h>
#include <signal.h>
#include <std_msgs/String.h>
#include <tf/transform_broadcaster.h>
#include "ros/ros.h"

#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
#include <webots_ros/set_bool.h>
#define TIME_STEP 32;

ros::NodeHandle *n;

static std::vector<float> lidarValues;

static int controllerCount;
static std::vector<std::string> controllerList;

ros::ServiceClient timeStepClient;
webots_ros::set_int timeStepSrv;

static int lms291Resolution = 0;
static int halfResolution = 0;
static double maxRange = 0.0;
static double rangeThreshold = 0.0;
static std::vector<double> braitenbergCoefficients;
static bool areBraitenbergCoefficientsinitialized = false;

//此处变量定义

// gaussian function
double gaussian(double x, double mu, double sigma)
{
  return (1.0 / (sigma * sqrt(2.0 * M_PI))) * exp(-((x - mu) * (x - mu)) / (2 * sigma * sigma));
}

//viod updateSpeed()


//tf发布
void broadcastTransform() {
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  //transform.setOrigin(tf::Vector3(-GPSValues[2], GPSValues[0], GPSValues[1]));
  //tf::Quaternion q(inertialUnitValues[0], inertialUnitValues[1], inertialUnitValues[2], inertialUnitValues[3]);
  //q = q.inverse();
  //transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "base_link"));
  transform.setIdentity();
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "/first_webots_ros_simulation/lidar3d"));
}


void lidar3dCallback(const sensor_msgs::LaserScan::ConstPtr &scan)
{
  int scanSize = scan->ranges.size();
  lidarValues.resize(scanSize);
  for (int i = 0; i < scanSize; ++i)
    lidarValues[i] = scan->ranges[i];

  lms291Resolution = scanSize;
  halfResolution = scanSize / 2;
  maxRange = scan->range_max;
  rangeThreshold = maxRange / 20.0;
  if (!areBraitenbergCoefficientsinitialized)
  {
    braitenbergCoefficients.resize(lms291Resolution);
    for (int i = 0; i < lms291Resolution; ++i)
      braitenbergCoefficients[i] = gaussian(i, halfResolution, lms291Resolution / 5);
    areBraitenbergCoefficientsinitialized = true;
  }

  //updateSpeed();
}

// catch names of the controllers availables on ROS network
void controllerNameCallback(const std_msgs::String::ConstPtr &name)
{
  controllerCount++;
  controllerList.push_back(name->data);
  ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
}

void quit(int sig)
{
  timeStepSrv.request.value = 0;
  timeStepClient.call(timeStepSrv);
  ROS_INFO("User stopped the 'webots_ros_controller' node.");
  ros::shutdown();
  exit(0);
}

int main(int argc, char **argv)
{
  std::string controllerName;
  // create a node named 'first_webots_ros_simulation' on ROS network
  ros::init(argc, argv, "first_webots_ros_simulation", ros::init_options::AnonymousName);
  n = new ros::NodeHandle;

  signal(SIGINT, quit);

  // subscribe to the topic model_name to get the list of availables controllers
  ros::Subscriber nameSub = n->subscribe("model_name", 100, controllerNameCallback);
  while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers())
  {
    ros::spinOnce();
    ros::spinOnce();
    ros::spinOnce();
  }
  ros::spinOnce();

  timeStepClient = n->serviceClient<webots_ros::set_int>("first_webots_ros_simulation/robot/time_step");
  timeStepSrv.request.value = TIME_STEP;

  // if there is more than one controller available, let the user choose
  if (controllerCount == 1)
    controllerName = controllerList[0];
  else
  {
    int wantedController = 0;
    std::cout << "Choose the # of the controller you want to use:\n";
    std::cin >> wantedController;
    if (1 <= wantedController && wantedController <= controllerCount)
      controllerName = controllerList[wantedController - 1];
    else
    {
      ROS_ERROR("Invalid number for  controller choice.");
      return 1;
    }
  }
  // leave topic once it's not necessary anymore
  nameSub.shutdown();

  // enable lidar3d
  ros::ServiceClient set_lidar3d_client;  //激光雷达
  ros::ServiceClient set_lidar3d1_client; //3d点云
  webots_ros::set_int lidar3d_srv;
  webots_ros::set_bool lidar3d1_srv;
  ros::Subscriber sub_lidar3d_scan;

  set_lidar3d_client = n->serviceClient<webots_ros::set_int>("first_webots_ros_simulation/lidar3d/enable");
  set_lidar3d1_client = n->serviceClient<webots_ros::set_bool>("first_webots_ros_simulation/lidar3d/enable_point_cloud");
  lidar3d_srv.request.value = TIME_STEP;
  lidar3d1_srv.request.value = true;
  if (set_lidar3d_client.call(lidar3d_srv) && lidar3d_srv.response.success)
  {
    ROS_INFO("lidar3d enabled.");
    sub_lidar3d_scan = n->subscribe("first_webots_ros_simulation/lidar3d/point_cloud", 10, lidar3dCallback);
    ROS_INFO("Topic for lidar3d initialized.");
    while (sub_lidar3d_scan.getNumPublishers() == 0)
    {
    }
    ROS_INFO("Topic for lidar3d scan connected.");
    if (set_lidar3d1_client.call(lidar3d1_srv) && lidar3d1_srv.response.success)
      ROS_INFO("lidar3d pointclouds enabled .");
  }
  else
  {
    if (!lidar3d_srv.response.success)
      ROS_ERROR("Sampling period is not valid.");
    ROS_ERROR("Failed to enable lidar3d.");
    return 1;
  }

  // enable camera
  ros::ServiceClient set_camera_client;
  webots_ros::set_int camera_srv;
  ros::Subscriber sub_camera;
  set_camera_client = n->serviceClient<webots_ros::set_int>("first_webots_ros_simulation/camera/enable");
  camera_srv.request.value = 64;
  set_camera_client.call(camera_srv);

  // main loop
  while (ros::ok())
  {
    if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success)
    {
      ROS_ERROR("Failed to call service time_step for next step.");
      break;
    }
    ros::spinOnce();
  }
  timeStepSrv.request.value = 0;
  timeStepClient.call(timeStepSrv);

  ros::shutdown();
  return 0;
}